#include "WProgram.h"

char * Version = "Arduino2WD01_01";

// LCD pins
const int LCD_RS = 0;
const int LCD_EN = 1;
const int LCD_D4 = 16;
const int LCD_D5 = 17;
const int LCD_D6 = 18;
const int LCD_D7 = 19;


LiquidCrystal lcd(LCD_RS, LCD_EN, LCD_D4, LCD_D5, LCD_D6, LCD_D7);

void LcdSetup()
{
  lcd.begin(16, 2);
}

// Motor pins

// Motor 1
const int Motor1_PIN1 = 5;
const int Motor1_PIN2 = 6;
const int Motor1_PWM = 9;
// Motor 2
const int Motor2_PIN1 = 3;
const int Motor2_PIN2 = 8;
const int Motor2_PWM = 10;

void MotorSetup()
{
        //TCCR1B = TCCR1B & 0b11111000 | 0x03;
  
	pinMode(Motor1_PWM, OUTPUT);
	digitalWrite(Motor1_PWM, LOW);

	pinMode(Motor1_PIN1, OUTPUT);
	pinMode(Motor1_PIN2, OUTPUT);
	
	pinMode(Motor2_PWM, OUTPUT);
	digitalWrite(Motor2_PWM, LOW);

	pinMode(Motor2_PIN1, OUTPUT);
	pinMode(Motor2_PIN2, OUTPUT);
}

void MotorStop()
{
	digitalWrite(Motor1_PWM, LOW);
	digitalWrite(Motor2_PWM, LOW);
	delay(1);
}


void MotorForward(int Speed1, int Speed2)
{
	MotorStop();

	digitalWrite(Motor1_PIN1, HIGH);
	digitalWrite(Motor1_PIN2, LOW);
	digitalWrite(Motor2_PIN1, HIGH);
	digitalWrite(Motor2_PIN2, LOW);
	
	//digitalWrite(Motor1_PWM, HIGH);
	//digitalWrite(Motor2_PWM, HIGH);
	
	delay(1);
	analogWrite(Motor2_PWM, Speed2);
	analogWrite(Motor1_PWM, Speed1);
}

void MotorBackward(int Speed1, int Speed2)
{
	MotorStop();
	
	digitalWrite(Motor1_PIN1, LOW);
	digitalWrite(Motor1_PIN2, HIGH);
	digitalWrite(Motor2_PIN1, LOW);
	digitalWrite(Motor2_PIN2, HIGH);
	
	delay(1);
	analogWrite(Motor2_PWM, Speed2);
	analogWrite(Motor1_PWM, Speed1);
}

void MotorLeft(int Speed1, int Speed2)
{
	MotorStop();

	digitalWrite(Motor1_PIN1, HIGH);
	digitalWrite(Motor1_PIN2, LOW);
	digitalWrite(Motor2_PIN1, LOW);
	digitalWrite(Motor2_PIN2, HIGH);
	
	delay(1);
	analogWrite(Motor2_PWM, Speed2);
	analogWrite(Motor1_PWM, Speed1);
}

void MotorRight(int Speed1, int Speed2)
{
	MotorStop();

	digitalWrite(Motor1_PIN1, LOW);
	digitalWrite(Motor1_PIN2, HIGH);
	digitalWrite(Motor2_PIN1, HIGH);
	digitalWrite(Motor2_PIN2, LOW);
	
	delay(1);
	analogWrite(Motor2_PWM, Speed2);
	analogWrite(Motor1_PWM, Speed1);
}

void Arduino2WD01_Setup()
{
	LcdSetup();
	MotorSetup();
}

